using System;
using System.Collections;
using System.Drawing;
using FIRADriverLibrary;
using Geometry;



namespace PotentialFieldDriver
{
	public abstract class PotencialDriver
	{
		public static ArrayList additionalObstaclePoints = new ArrayList();
		public static ArrayList walls = new ArrayList();

		public static ArrayList angleLocks;
		public static Point2D checkPoint;

		public const double wheelsHalfDistance = 0.034;
		public const double robotCollisionAvoidanceDistance = 0.2;

		public static double maxVelocity = 1.0;
		public static double tinyVelocity = 0.3;	//below robot will change direction if needed, above will turn
		public static double freeCurveFactor = 0.4;
		public static double collisionDetectionTime = 0.5;

        private static Point2D[] targets = new Point2D[] { new Point2D(0.5,0.5)};
        private static int targetCounter = 0;
		
		//public static double collisionAvoidanceFactor = 2;

		public static FIRADriver firaDriver;
		public static RectangleF pitchArea;

		public static void DriveEta (FIRARobotState robot, double x, double y, double eta)
		{
            
            x = targets[targetCounter].x;
            y = targets[targetCounter].y;
            //if(new Vector2D(new Point2D(robot.x,robot.y), new Point2D(x,y).Length<0.3)
            //{
            //}
            x = targets[targetCounter].x;
            y = targets[targetCounter].y;
            DriveVelocity(robot, x, y, new Vector2D(x - robot.x, y - robot.y).Length / eta);
			//DriveVelocity(robot, x, y, new Vector2D(x-robot.x, y-robot.y).Length / eta);
		}

		public static void DriveVelocity (FIRARobotState robot, double x, double y, double velocity)
		{
			double linearVel = (robot.leftSpeed + robot.rightSpeed) / 2;
			double deltaVel = (robot.leftSpeed - robot.rightSpeed) / 2;

			checkPoint = CalculateCollisionFreeCheckPoint(robot, x, y, Math.Abs(collisionDetectionTime * linearVel) + robotCollisionAvoidanceDistance);
			linearVel = (robot.leftSpeed + robot.rightSpeed) / 2;	//might have chaged
			deltaVel = (robot.leftSpeed - robot.rightSpeed) / 2;
			if (checkPoint.x != 0.0 || checkPoint.y != 0.0)
			{
				double toCheckPointDistance = Vector2D.GetLength(robot.x - checkPoint.x, robot.y - checkPoint.y);
				double fromCheckPointDistance = Vector2D.GetLength(x - checkPoint.x, y - checkPoint.y);

				CalculateControlNoObstacles(robot, checkPoint.x, checkPoint.y, velocity
					, ref linearVel, ref deltaVel);
			}
			else
			{
				CalculateControlNoObstacles(robot, x, y, velocity, ref linearVel, ref deltaVel);
				angleLocks.Clear();
			}
            
			robot.leftSpeed = linearVel + deltaVel;
			robot.rightSpeed = linearVel - deltaVel;
		}


		private static void CalculateControlNoObstacles(FIRARobotState robot, double x, double y, double velocity, ref double linearVel, ref double deltaVel)
		{
			Vector2D robotVersor = new Vector2D(robot.angle);
			Vector2D targetVector = new Vector2D(x-robot.x, y-robot.y);

			double angleRobotTarget = Vector2D.AngleBetween(robotVersor, targetVector);

			linearVel = Math.Sign(Math.Cos(angleRobotTarget)) * Math.Min(maxVelocity, velocity);
			deltaVel = freeCurveFactor * Math.Sin(angleRobotTarget) * linearVel;

			if (targetVector.Length < robotCollisionAvoidanceDistance)	// sharpen turn if close to target
			{
				double sharpenFactor = (robotCollisionAvoidanceDistance - targetVector.Length) / robotCollisionAvoidanceDistance;
				deltaVel *= 1 + sharpenFactor;
				linearVel *= 0.5 + 0.5*(1 - sharpenFactor);
			}

			if (Math.Abs(robot.leftSpeed + robot.rightSpeed) / 2 > tinyVelocity)	// robot at motion
			{
				if (Math.Sign(robot.leftSpeed + robot.rightSpeed) != Math.Sign(linearVel))	// invert motion
				{
					linearVel = -linearVel;
					deltaVel = Math.Sign(Math.Sin(angleRobotTarget)) * freeCurveFactor * linearVel;
				}
			}
		}

		public class AngleLock
		{
			public Vector2D lockVector;
			public double halfAngle;

			public AngleLock (double lockCenterAngle, double halfAngle, double distance)
			{
				this.lockVector = new Vector2D(lockCenterAngle);
				this.lockVector *= distance;
				this.halfAngle = halfAngle;
			}

			public bool TryJoin(AngleLock secondLock)
			{
				double angleBetween = Vector2D.AngleBetween(lockVector, secondLock.lockVector);

				if (Math.Abs(angleBetween) > halfAngle + secondLock.halfAngle)
					return false;

				/// the second range can exceed the first from one or both sides
				double wider_big = Math.Abs(angleBetween) + secondLock.halfAngle - halfAngle;
				double wider_small = - Math.Abs(angleBetween) + secondLock.halfAngle - halfAngle;
				if (wider_big > 0)
				{
					if (wider_small < 0)	
						wider_small = 0;
					double length = Math.Min(lockVector.Length, secondLock.lockVector.Length);
					lockVector = new Vector2D(lockVector.Angle + Math.Sign(angleBetween) * (wider_big - wider_small) / 2);
					this.lockVector *= length;
					this.halfAngle += (wider_big + wider_small) / 2;
					this.halfAngle = Math.Min(this.halfAngle, Math.PI);
				}
				return true;
			}

			public double GetClosestFreeAngle(double angle)
			{
				Vector2D robotVersor = new Vector2D(angle);
				double angleBetween = Vector2D.AngleBetween(lockVector, robotVersor);
				if (Math.Abs(angleBetween) > this.halfAngle)
					return angle;

				return lockVector.Angle + Math.Sign(angleBetween) * this.halfAngle;
			}
		}
		
		private static Point2D CalculateCollisionFreeCheckPoint(FIRARobotState robot, double x, double y, double lookupDistance)
		{

			Vector2D targetVector = new Vector2D(x-robot.x, y-robot.y);
			Vector2D robotVector = targetVector + (new Vector2D(robot.angle) * (lookupDistance  * Math.Sign(robot.leftSpeed + robot.rightSpeed)));
			double robotAngle = robotVector.Angle;

			angleLocks = new ArrayList();
			Vector2D robotObstacleRobotVector;
            		
			// robots locks
			foreach (FIRARobotState obstacleRobot in firaDriver.firaRobots)
			{
				if (obstacleRobot == robot)
					continue;

				robotObstacleRobotVector.X = obstacleRobot.x - robot.x;
				robotObstacleRobotVector.Y = obstacleRobot.y - robot.y;
				double obstacleDistance = robotObstacleRobotVector.Length;
				if (obstacleDistance > lookupDistance)
					continue;

				double obstacleAngle = robotObstacleRobotVector.Angle;
				double obstacleWidthHalfAngle = Math.Atan(robotCollisionAvoidanceDistance / obstacleDistance);
				if (obstacleDistance < robotCollisionAvoidanceDistance)
				obstacleWidthHalfAngle *= 1 + (robotCollisionAvoidanceDistance - obstacleDistance) / robotCollisionAvoidanceDistance;
				angleLocks.Add(new AngleLock(obstacleAngle, obstacleWidthHalfAngle, obstacleDistance));
			}

			// additional points locks
			lock (additionalObstaclePoints)
			{
				foreach (Point2D obstaclePoint in additionalObstaclePoints)
				{
					robotObstacleRobotVector.X = obstaclePoint.x - robot.x;
					robotObstacleRobotVector.Y = obstaclePoint.y - robot.y;
					double obstacleDistance = robotObstacleRobotVector.Length;
					if (obstacleDistance > lookupDistance)
						continue;

					double obstacleAngle = robotObstacleRobotVector.Angle;
					double obstacleWidthHalfAngle = Math.Atan(robotCollisionAvoidanceDistance / obstacleDistance);
					if (obstacleDistance < robotCollisionAvoidanceDistance)
						obstacleWidthHalfAngle *= 1 + (robotCollisionAvoidanceDistance - obstacleDistance) / robotCollisionAvoidanceDistance;
					angleLocks.Add(new AngleLock(obstacleAngle, obstacleWidthHalfAngle, obstacleDistance));
				}
			}
			

			// walls locks
			AngleLock wallLock = null;
			lock (walls)
			{
				foreach (Wall2D wall in walls )
				{
					wallLock = wall.GetLockForRobot(robot.x, robot.y, lookupDistance);
					if (wallLock != null)
						angleLocks.Add(wallLock);
				}
			}


			//remove too far locks
			for (int i = 0 ; i < angleLocks.Count ; i++)
			{
				if (((AngleLock)angleLocks[i]).lockVector.Length > targetVector.Length)	 
				{
					angleLocks.RemoveAt(i);
					i--;
				}
			}

			///join locks
			for (int i = 0 ; i < angleLocks.Count ; i++)
			{
				AngleLock angleLock = (AngleLock)angleLocks[i];
				if (angleLock.lockVector.Length > targetVector.Length)	 
				{
					angleLocks.RemoveAt(i);
					i--;
				}
				for (int j = i + 1 ; j < angleLocks.Count ; j++)
				{
					if (angleLock.TryJoin((AngleLock)angleLocks[j]))
					{
						angleLocks.RemoveAt(j);
						j--;
					}
				}
			}

			// find and go around
			double newAngle = robotAngle;
			foreach (AngleLock angleLock in angleLocks)
			{
					
				newAngle = angleLock.GetClosestFreeAngle(newAngle);
				if (newAngle != robotAngle)
				{
					if (angleLock.lockVector.Length < robotCollisionAvoidanceDistance) //prevent keeping motion direction
					{						
						robot.leftSpeed = 0.0;
						robot.rightSpeed = 0.0;
					}
					return new Point2D(robot.x + angleLock.lockVector.Length * Math.Cos(newAngle),
						robot.y + angleLock.lockVector.Length * Math.Sin(newAngle));
				}
			}
			return new Point2D(0.0, 0.0);

		}
	}
}
